﻿#pragma once
#include <QObject>
#include "../algorithmvisual.h"
#include "visual_data.h"
#include <qmath.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/common/transforms.h>
#include <pcl/common/centroid.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/types_c.h>


using namespace unstack;
class AlgorithmUnstackCommon : public AlgorithmVisual
{
public:
	AlgorithmUnstackCommon();

	~AlgorithmUnstackCommon();

	virtual int Init(const visAlgParam& param) override;

	virtual bool Caculate(const VisAlgInput& input, Position& pos, VisAlgOutput& out) override;

private:
	// 解析识别结果
	int ParseRecogRecogResult(const QList<BoxRect>& input, QVector<RECGRESULT>& result);


	PointCloud::Ptr pix2world(PointCloud::Ptr cloud);
	PointCloud::Ptr GetJigPoint(const JigSetting& jig);

	int PointCloudInterferenceCheck(BoxInfo& recog, PointCloud::Ptr sucker_convex, PointCloud::Ptr cloud_filter_ptr, float& delx, float& dely, cv::Mat& image);

	int getPointIntersection(PointCloud::Ptr point_hull, PointCloud::Ptr point_source);

	BoxInfo GetCaseInfoFromPix(PointCloud::Ptr cloud_case, RECGRESULT recg);

	BoxInfo GetCaseInfoFromPix2(PointCloud::Ptr cloud_case, RECGRESULT recg);

	PointCloud::Ptr Filter(PointCloud::Ptr cloud_in);

	QVector<PointCloud::Ptr> GetCaseCloudVec(PointCloud::Ptr cloud);

	PointCloud::Ptr GetCloudWithoutCase(BoxInfo& caseinfo, PointCloud::Ptr cloud_ptr_world);

	PointCloud::Ptr GetCloudWithoutCaseVec(QVector<BoxInfo>& caseinfo_vec, PointCloud::Ptr cloud_ptr_world);

	PointCloud::Ptr GetCloudWithCase(BoxInfo& caseinfo, PointCloud::Ptr cloud_ptr_world);

	PointCloud::Ptr GetCloudWithCaseVec(QVector<BoxInfo>& caseinfo_vec, PointCloud::Ptr cloud_ptr_world);

	QVector<QVector<BoxInfo>> FindNeatCase(QVector<BoxInfo> const case_vec);

	// 多抓尝试，如果成功返回多抓数量
	int trySeveralGrab(QVector<QVector<BoxInfo>>& neatcase, PointCloud::Ptr cloud_world_ptr, const int max,
		const JigSetting& jig, GrabInfo& grab, BoxInfo& target);

	bool InitCalibration(const QString& file);	// 标定文件读取
	bool CheckParam(const VisAlgInput& input, QString& msg);	// 参数校验
	//更新上一次的数据  把本次需要拆垛的箱体的任意一个放入到结构体中  
	void UpdateBeforeinfo(BoxInfo& reinfo, int dx, int dy, RECORDINFO& info);

	//计算高度差
	//file  深度图地址
	//info  上一次拆垛更新之后的结构体数据
	//fhigh 计算之后的高度差
	void CalPointDifference(QString file, RECORDINFO info, QString& ipt);

	//根据深度图转化的点云和坐标   切割点云
	PointCloud::Ptr GetCaseCloud(PointCloud::Ptr cloud, RECGRESULT result);

	void EuclideanClusterExtractionCloud(const PointCloud::Ptr& pCloud, float fClusterTolerance, int iMinClusterSize, int iMaxClusterSize, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>* pvCloudPtr);

	double getMaxCloud(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> vCloudPtr);

	PointCloud::Ptr cam2pix(PointCloud::Ptr cloud);

	PointCloud::Ptr pix2cam(PointCloud::Ptr cloud);

	cv::RotatedRect getminAreaRect(cv::Mat Mask);

	void UpsampleMask(cv::Mat mask, cv::Mat& upMask, int pad);

	int BoxCheck(BoxInfo& recog, PointCloud::Ptr cloud_filter_ptr);

	bool GetBoxSize(QString arr);

	void BlockCheck(QVector<QVector<BoxInfo>>& neatcase, int casenum);

	bool getBlockInfo(QVector<BoxInfo>& block, BoxInfo& cases_one);

	float GetAngleTrans(float angle, float angleX, float angleY);

	void parrProcess(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr pass_cloud);

	void DrawPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, cv::Scalar sc, cv::Mat& image);

	void DrawTray(cv::Mat& image, double maxHeight);

	void DrowBox(cv::Mat& image, cv::RotatedRect rect);


	void DrowCoordinate(SKUPOSITION position, cv::Mat& rgb, float delx, float delty, int count);

	void DrowSucker(float angle, BoxInfo& recog, float deltx, float delty, cv::Mat& image);
	bool SortSkuPos(QVector<BoxInfo>& pos);
	// 获取抓取位姿
	QVector<GrabInfo> GetGrabPosVec(BoxInfo& recog, const JigSetting& jig);
	GrabInfo GetGrabPosition(BoxInfo& recog, const JigSetting& jig, PointCloud::Ptr world);

private:

	bool bSave;
	pcl::visualization::PCLVisualizer::Ptr viewer;
	RECORDINFO beforinfo;
	QVector<RECGRESULT> recog_result; // 识别结果，箱子框坐标

	QVector<QVector<float>> m_intri;
	QVector<QVector<float>> m_external;
	cv::Mat m_intriMatrix;//内参
	cv::Mat m_distorMatrix;//畸变系数
	cv::Mat m_exteMatrix;
	Position m_plateMin = {};
	Position m_plateMax = {};	// 托盘的坐标范围

	int boxLength, boxWidth, boxHeight = 0; //箱子长宽高，初始化为0

	int m_dir = -1;	// 拆垛方向 
	int m_grabdir = -1; // 货物抓取方式 0：平行抓取，1：垂直抓取
	//5.最多抓取个数
	int m_maxNum = 1;

	//7.是否启用箱子尺寸校验
	bool SizeCheck = true;

	//8.物料的最大最小尺寸
	int maxBoxLength = 1000;
	int maxBoxWidth = 1000;
	int minBoxLength = 0;
	int minBoxWidth = 0;

	struct
	{
		bool offset;
		float x_min;
		float y_min;
		float x_max;
		float y_max;
		float x_off;
		float y_off;
	} m_offsetInfo = {};
};
